Даны два облака точек. Найти жёсткое преобразование из одного в другое
Соответвие между начальными и конечными точками не известно
Данные имеют шум
Note
Жёсткое преобразование сохраняет расстояние между точками, т.е. поворот + параллельный перенос
Дано: \(X = \{ \mathbf{x}_1, \mathbf{x}_2, \dots, \mathbf{x}_N \} \subset \mathbb{R}^n\), \(Y = \{ \mathbf{y}_1, \mathbf{y}_2, \dots, \mathbf{y}_N \} \subset \mathbb{R}^n\)
\[ \mathbf{y}_{\pi(i)} = R \mathbf{x}_i + t + \boldsymbol{\epsilon}_i, \quad \forall i = 1, \dots, N, \]
где
\(\boldsymbol{\epsilon}_i \in \mathbb{R}^n\) — вектор шума для точки \(i\), \(\boldsymbol{\epsilon}_i \sim \mathcal{N}(0,\, (\sigma^2, \sigma^2,\sigma^2))\)
\(R \in SO(3)\) — матрица вращения \(R^T R = I\), \(\det(R) = 1\)
\(t \in \mathbb{R}^n\) — вектор смещения,
\(\pi: \{1, \dots, N\} \to \{1, \dots, N\}\) — перестановка индексов.
\[ \underset{R \in SO(3),\,t,\,\pi}{\arg \min}\ \sum_{i=1}^N \| R \mathbf{x}_i + t - \mathbf{y}_{\pi(i)} + \boldsymbol{\epsilon}_i \|^2, \]
\[ Y^T = R X^T P + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} + E, \]
\[ \underset{R \in SO(3),\,t,\,P}{\arg \min}\ \| R X^T P + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]
\[ \underset{R \in SO(3),\,t,\,P}{\arg \min}\ \| R X^TP + t \begin{pmatrix} 1 & . . . & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]
Ключевая сложность — поиск матрицы соответствия.
Целевая функция приобретает дискретную форму — обычные методы оптимизации не применимы
Алгоритм Кабша(-Умеямы) умеет решать успрощенную задачу с известным соответствием
\[ \underset{R \in SO(3),\,t}{\arg \min}\ \| R X^T + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]
Избавимся от матрицы перестановок — будем уменьшать расстояние между облаками.
Изменим целевую функцию
\[ \underset{{R \in SO(3),\ t}}{\arg \min}\ \sum_{i=1}^N \| R \mathbf{x}_i + t - \mathbf{y}_{\sigma(i)} \|^2 \]
Iteartive Closest Point — итеративный метод поиска жёсткого преобразования через сопоставление ближайших точек. В качестве приближенного соответсвия точек используется сопоставление каждой ближайшей к ней (из другого облака). Для полученного соответствия можно использовать алгоритм Кабша

Использование упрощенной метрики ведет к тому, что целевая функция преобретает значительное количество локальных минимумов. В связи с чем, использование алгоритма ограничено: ICP показывает хорошие результаты только при малых углах поворота